// *************************************************************************************************
//
// Horde3D
//   Next-Generation Graphics Engine
//
// Sample Application
// --------------------------------------
// Copyright (C) 2006-2011 Nicolas Schulz
//
//
// This sample source file is not covered by the EPL as the rest of the SDK
// and may be used without any restrictions. However, the EPL's disclaimer of
// warranty and liability shall be in effect for this file.
//
// *************************************************************************************************

#include "crowd.h"
#include <math.h>
#include <stdlib.h>
#include <time.h>
#include <map>




#ifndef M_PI
static const float M_PI = 3.14159265358979323846f;
#endif

void CrowdSim::buildRoadmap(RVO::RVOSimulator* sim)
{
	/* Connect the roadmap vertices by edges if mutually visible. */
#pragma omp parallel for
	for (int i = 0; i < static_cast<int>(roadmap.size()); ++i) {
		for (int j = 0; j < static_cast<int>(roadmap.size()); ++j) {
			if (sim->queryVisibility(roadmap[i].position, roadmap[j].position, sim->getAgentRadius(0))) {
				roadmap[i].neighbors.push_back(j);
			}
		}

		/* 
		* Initialize the distance to each of the four goal vertices at infinity
		* (9e9f).
		*/
		roadmap[i].distToGoal.resize(4, 9e9f);
	}

	/*
	* Compute the distance to each of the four goals (the first four vertices)
	* for all vertices using Dijkstra's algorithm.
	*/
#pragma omp parallel for
	for (int i = 0; i < 4; ++i) {
		std::multimap<float, int> Q;
		std::vector<std::multimap<float, int>::iterator> posInQ(roadmap.size(), Q.end());

		roadmap[i].distToGoal[i] = 0.0f;
		posInQ[i] = Q.insert(std::make_pair(0.0f, i));

		while (!Q.empty()) {
			const int u = Q.begin()->second;
			Q.erase(Q.begin());
			posInQ[u] = Q.end();

			for (int j = 0; j < static_cast<int>(roadmap[u].neighbors.size()); ++j) {
				const int v = roadmap[u].neighbors[j];
				const float dist_uv = RVO::abs(roadmap[v].position - roadmap[u].position);

				if (roadmap[v].distToGoal[i] > roadmap[u].distToGoal[i] + dist_uv) {
					roadmap[v].distToGoal[i] = roadmap[u].distToGoal[i] + dist_uv;

					if (posInQ[v] == Q.end()) {
						posInQ[v] = Q.insert(std::make_pair(roadmap[v].distToGoal[i], v));
					} else {
						Q.erase(posInQ[v]);
						posInQ[v] = Q.insert(std::make_pair(roadmap[v].distToGoal[i], v));
					}
				}
			}
		}
	}

}

void CrowdSim::init()
{
	sim = new RVO::RVOSimulator();
	
	// Init random generator
	//srand( (unsigned int)time( NULL ) );
	srand( 99777 );  // Use fixed value for better performance comparisons
	
	/* Specify the global time step of the simulation. */
	sim->setTimeStep(0.25f);
	// Add Obstacles
	std::vector<RVO::Vector2> obstacle1, obstacle2, obstacle3, obstacle4, obstacle5, obstacle6, obstacle7, obstacle8, obstacle9, obstacle10, obstacle11, obstacle12, obstacle13, obstacle14, obstacle15, obstacle16, obstacle17 ;

	obstacle1.push_back(RVO::Vector2(-51.0f, -50.0f));
	obstacle1.push_back(RVO::Vector2(-51.0f, -51.0f));
	obstacle1.push_back(RVO::Vector2(51.0f, -51.0f));
	obstacle1.push_back(RVO::Vector2(51.0f, -50.0f));

	obstacle2.push_back(RVO::Vector2(-23.0f, 50.0f));
	obstacle2.push_back(RVO::Vector2(-23.0f, 51.0f));
	obstacle2.push_back(RVO::Vector2(51.0f, 51.0f));
	obstacle2.push_back(RVO::Vector2(51.0f, 50.0f));

	obstacle17.push_back(RVO::Vector2(-27.0f, 50.0f));
	obstacle17.push_back(RVO::Vector2(-27.0f, 51.0f));
	obstacle17.push_back(RVO::Vector2(-51.0f, 51.0f));
	obstacle17.push_back(RVO::Vector2(-51.0f, 50.0f));

	obstacle3.push_back(RVO::Vector2(-50.0f, -50.0f));
	obstacle3.push_back(RVO::Vector2(-51.0f, -50.0f));
	obstacle3.push_back(RVO::Vector2(-51.0f, 50.0f));
	obstacle3.push_back(RVO::Vector2(-50.0f, 50.0f));

	obstacle4.push_back(RVO::Vector2(50.0f, -50.0f));
	obstacle4.push_back(RVO::Vector2(51.0f, -50.0f));
	obstacle4.push_back(RVO::Vector2(51.0f, 50.0f));
	obstacle4.push_back(RVO::Vector2(50.0f, 50.0f));
	
	//
	obstacle5.push_back(RVO::Vector2(-50.0f, 0.5f));
	obstacle5.push_back(RVO::Vector2(-50.0f, -0.5f));
	obstacle5.push_back(RVO::Vector2(-12.0f, -0.5f));
	obstacle5.push_back(RVO::Vector2(-12.0f, 0.5f));

	obstacle6.push_back(RVO::Vector2(-8.0f, 0.5f));
	obstacle6.push_back(RVO::Vector2(-8.0f, -0.5f));
	obstacle6.push_back(RVO::Vector2(50.0f, -0.5f));
	obstacle6.push_back(RVO::Vector2(50.0f, 0.5f));

	obstacle7.push_back(RVO::Vector2(-50.0f, 0.5f));
	obstacle7.push_back(RVO::Vector2(-50.0f, -0.5f));
	obstacle7.push_back(RVO::Vector2(-25.5f, -0.5f));
	obstacle7.push_back(RVO::Vector2(-25.5f, 0.5f));

	obstacle8.push_back(RVO::Vector2(-50.0f, -14.5f));
	obstacle8.push_back(RVO::Vector2(-50.0f, -15.5f));
	obstacle8.push_back(RVO::Vector2(-25.5f, -15.5f));
	obstacle8.push_back(RVO::Vector2(-25.5f, -14.5f));

	obstacle9.push_back(RVO::Vector2(-50.0f, -29.5f));
	obstacle9.push_back(RVO::Vector2(-50.0f, -30.5f));
	obstacle9.push_back(RVO::Vector2(-25.5f, -30.5f));
	obstacle9.push_back(RVO::Vector2(-25.5f, -29.5f));

	obstacle10.push_back(RVO::Vector2(-50.0f, 15.5f));
	obstacle10.push_back(RVO::Vector2(-50.0f, 14.5f));
	obstacle10.push_back(RVO::Vector2(-25.5f, 14.5f));
	obstacle10.push_back(RVO::Vector2(-25.5f, 15.5f));

	obstacle11.push_back(RVO::Vector2(-50.0f, 30.5f));
	obstacle11.push_back(RVO::Vector2(-50.0f, 29.5f));
	obstacle11.push_back(RVO::Vector2(-25.5f, 29.5f));
	obstacle11.push_back(RVO::Vector2(-25.5f, 30.5f));
	
	//
	obstacle12.push_back(RVO::Vector2(-24.5f, -50.0f));
	obstacle12.push_back(RVO::Vector2(-25.5f, -50.0f));
	obstacle12.push_back(RVO::Vector2(-25.5f, -47.0f));
	obstacle12.push_back(RVO::Vector2(-24.5f, -47.0f));

	obstacle13.push_back(RVO::Vector2(-24.5f, -43.0f));
	obstacle13.push_back(RVO::Vector2(-25.5f, -43.0f));
	obstacle13.push_back(RVO::Vector2(-25.5f, -25.0f));
	obstacle13.push_back(RVO::Vector2(-24.5f, -25.0f));

	obstacle14.push_back(RVO::Vector2(-24.5f, -21.0f));
	obstacle14.push_back(RVO::Vector2(-25.5f, -21.0f));
	obstacle14.push_back(RVO::Vector2(-25.5f, 4.0f));
	obstacle14.push_back(RVO::Vector2(-24.5f, 4.0f));

	obstacle15.push_back(RVO::Vector2(-24.5f, 8.0f));
	obstacle15.push_back(RVO::Vector2(-25.5f, 8.0f));
	obstacle15.push_back(RVO::Vector2(-25.5f, 21.0f));
	obstacle15.push_back(RVO::Vector2(-24.5f, 21.0f));

	obstacle16.push_back(RVO::Vector2(-24.5f, 25.0f));
	obstacle16.push_back(RVO::Vector2(-25.5f, 25.0f));
	obstacle16.push_back(RVO::Vector2(-25.5f, 30.5f));
	obstacle16.push_back(RVO::Vector2(-24.5f, 30.5f));

	sim->addObstacle(obstacle1);
	sim->addObstacle(obstacle2);
	sim->addObstacle(obstacle3);
	sim->addObstacle(obstacle4);
	sim->addObstacle(obstacle5);
	sim->addObstacle(obstacle6);
	sim->addObstacle(obstacle7);
	sim->addObstacle(obstacle8);
	sim->addObstacle(obstacle9);
	sim->addObstacle(obstacle10);
	sim->addObstacle(obstacle11);
	sim->addObstacle(obstacle12);
	sim->addObstacle(obstacle13);
	sim->addObstacle(obstacle14);
	sim->addObstacle(obstacle15);
	sim->addObstacle(obstacle16);
	sim->addObstacle(obstacle17);

	/* Process the obstacles so that they are accounted for in the simulation. */
	sim->processObstacles();
	
	/* Add roadmap vertices. */
	RoadmapVertex v;

	/* Add the goal positions of agents. */
	v.position = RVO::Vector2(-75.0f, 75.0f);
	roadmap.push_back(v);
	v.position = RVO::Vector2(-40.0f, 40.0f);
	roadmap.push_back(v);
	v.position = RVO::Vector2(-40.0f, 22.0f);
	roadmap.push_back(v);
	v.position = RVO::Vector2(-40.0f, 7.0f);
	roadmap.push_back(v);

	/* Add roadmap vertices around the obstacles. */
	v.position = RVO::Vector2(-22.0f, -45.0f);
	roadmap.push_back(v);
	v.position = RVO::Vector2(-28.0f, -45.0f);
	roadmap.push_back(v);

	v.position = RVO::Vector2(-22.0f, -23.0f);
	roadmap.push_back(v);
	v.position = RVO::Vector2(-28.0f, -23.0f);
	roadmap.push_back(v);

	v.position = RVO::Vector2(-22.0f, -6.0f);
	roadmap.push_back(v);
	v.position = RVO::Vector2(-28.0f, -6.0f);
	roadmap.push_back(v);

	v.position = RVO::Vector2(-22.0f, 6.0f);
	roadmap.push_back(v);
	v.position = RVO::Vector2(-28.0f, 6.0f);
	roadmap.push_back(v);

	v.position = RVO::Vector2(-22.0f, 23.0f);
	roadmap.push_back(v);
	v.position = RVO::Vector2(-28.0f, 23.0f);
	roadmap.push_back(v);

	v.position = RVO::Vector2(-10.0f, -3.0f);
	roadmap.push_back(v);
	v.position = RVO::Vector2(-10.0f, 3.0f);
	roadmap.push_back(v);

	v.position = RVO::Vector2(-25.0f, 53.0f);
	roadmap.push_back(v);
	v.position = RVO::Vector2(-25.0f, 47.0f);
	roadmap.push_back(v);

	/* Specify the default parameters for agents that are subsequently added. */
	sim->setAgentDefaults(5.0f, 10, 0.5f, 0.5f, 0.3f, 0.6f);

	sim->addAgent(RVO::Vector2(-28.0f, 6.0f));
	goals.push_back(0);
// 	H3DNode tmp0 = h3dAddNodes(H3DRootNode, characterRes);
// 	h3dSetupModelAnimStage( tmp0, 0, characterWalkRes, 0, "", false );
// 	h3dSetNodeTransform( tmp0, -28.0f, 0.02f, 6.0f, 0, 0, 0, 1, 1, 1 );

	nodes.push_back(tmp0);
	animTime.push_back(0);

	// Add characters
	for (int i = 0; i < 5; ++i) {
		for (int j = 0; j < 5; ++j) {
			sim->addAgent(RVO::Vector2(-40.0f + i * 1.0f, 40.0f + j * 1.0f));
			goals.push_back(0);
			H3DNode tmp = h3dAddNodes(H3DRootNode, characterRes);
			h3dSetupModelAnimStage( tmp, 0, characterWalkRes, 0, "", false );
			h3dSetNodeTransform( tmp, -40.0f + i * 1.0f, 0.02f, 40.0f + j * 1.0f, 0, 0, 0, 1, 1, 1 );
			nodes.push_back(tmp);
			animTime.push_back(0);

			sim->addAgent(RVO::Vector2(-40.0f - i * 1.0f, 22.0f + j * 1.0f));
			goals.push_back(0);
			H3DNode tmp1 = h3dAddNodes(H3DRootNode, characterRes);
			h3dSetupModelAnimStage( tmp1, 0, characterWalkRes, 0, "", false );
			h3dSetNodeTransform( tmp1, -40.0f - i * 1.0f, 0.02f, 22.0f + j * 1.0f, 0, 0, 0, 1, 1, 1 );
			nodes.push_back(tmp1);
			animTime.push_back(0);

			sim->addAgent(RVO::Vector2(-40.0f + i * 1.0f, 7.0f - j * 1.0f));
			goals.push_back(0);
			H3DNode tmp2 = h3dAddNodes(H3DRootNode, characterRes);
			h3dSetupModelAnimStage( tmp2, 0, characterWalkRes, 0, "", false );
			h3dSetNodeTransform( tmp2, -40.0f + i * 1.0f, 0.02f, 7.0f - j * 1.0f, 0, 0, 0, 1, 1, 1 );
			nodes.push_back(tmp2);
			animTime.push_back(0);
		}
	}

	buildRoadmap(sim);
}

bool CrowdSim::reachedGoal(RVO::RVOSimulator* sim)
{
	/* Check if all agents have reached their goals. */
	for (size_t i = 0; i < sim->getNumAgents(); ++i) {
		if (RVO::absSq(sim->getAgentPosition(i) - roadmap[goals[i]].position) > 20.0f * 20.0f) {
			return false;
		}
	}

	return true;
}

void CrowdSim::setPreferredVelocities(RVO::RVOSimulator* sim)
{
	/*
	* Set the preferred velocity to be a vector of unit magnitude (speed) in the
	* direction of the visible roadmap vertex that is on the shortest path to the
	* goal.
	*/
#pragma omp parallel for
	for (int i = 0; i < static_cast<int>(sim->getNumAgents()); ++i) {
		float minDist = 9e9f;
		int minVertex = -1;

		for (int j = 0; j < static_cast<int>(roadmap.size()); ++j) {
			if (RVO::abs(roadmap[j].position - sim->getAgentPosition(i)) + roadmap[j].distToGoal[goals[i]] < minDist &&
				sim->queryVisibility(sim->getAgentPosition(i), roadmap[j].position, sim->getAgentRadius(i))) {

					minDist = RVO::abs(roadmap[j].position - sim->getAgentPosition(i)) + roadmap[j].distToGoal[goals[i]];
					minVertex = j;
			}
		}

		if (minVertex == -1) {
			/* No roadmap vertex is visible; should not happen. */
			sim->setAgentPrefVelocity(i, RVO::Vector2(0, 0));
		} else {
			if (RVO::absSq(roadmap[minVertex].position -
				sim->getAgentPosition(i)) == 0.0f) {
					if (minVertex == goals[i]) {
						sim->setAgentPrefVelocity(i, RVO::Vector2());
					} else {
						sim->setAgentPrefVelocity(i, RVO::normalize(roadmap[goals[i]].position - sim->getAgentPosition(i)));
					}
			} else {
				sim->setAgentPrefVelocity(i, RVO::normalize(roadmap[minVertex].position - sim->getAgentPosition(i)));
			}
		}

		/*
		* Perturb a little to avoid deadlocks due to perfect symmetry.
		*/
		float angle = std::rand() * 2.0f * M_PI / RAND_MAX;
		float dist = std::rand() * 0.0001f / RAND_MAX;

		sim->setAgentPrefVelocity(i, sim->getAgentPrefVelocity(i) + 
			dist * RVO::Vector2(std::cos(angle), std::sin(angle)));
	}
}


void CrowdSim::updateVisualization(RVO::RVOSimulator* sim)
{


	/* Output the current position of all the agents. */
	for (int i = 0; i < sim->getNumAgents(); ++i) {
		//  std::cout << sim->getAgentPosition( i ) << " " << sim->getAgentOrientation( i ) << " ";
		float ry = 0;
		if( sim->getAgentPrefVelocity(i).y() != 0 ) ry = atan2( sim->getAgentVelocity(i).x(), sim->getAgentVelocity(i).y() );
		ry *= 180 / 3.1415f;	// Convert from radians to degrees
		h3dSetNodeTransform( nodes[i], sim->getAgentPosition( i ).x(), 0.02f, sim->getAgentPosition( i ).y(), 0, ry, 0, 1, 1, 1 );
		float vel = sqrtf( sim->getAgentVelocity(i).x() * sim->getAgentVelocity(i).x() + sim->getAgentVelocity(i).y() * sim->getAgentVelocity(i).y() );
		animTime[i] += vel * 10.0f;
		h3dSetModelAnimParams( nodes[i], 0, animTime[i], 1.0f );
	}

}

void CrowdSim::update( float fps )
{
	if(reachedGoal(sim)) return;
	updateVisualization(sim);
	setPreferredVelocities(sim);
	sim->doStep();




#if 0
	// Parameters for three repulsion zones
	float d1 = 0.25f, d2 = 2.0f, d3 = 4.5f;
	float f1 = 3.0f, f2 = 1.0f, f3 = 0.1f;
	for( unsigned int i = 0; i < _particles.size(); ++i )
	{
		Particle &p = _particles[i];
		
		// Reset force
		p.fx = 0; p.fz = 0;
		
		// Calculate distance to destination
		float dist = sqrtf( (p.dx - p.px)*(p.dx - p.px) + (p.dz - p.pz)*(p.dz - p.pz) );
		
		// If destination not reached walk to destination
		if( dist > 3.0f )
		{
			// Calculate normalized attraction force to destination
			float afx = (p.dx - p.px) / dist;
			float afz = (p.dz - p.pz) / dist;	

			p.fx += afx * 0.035f; p.fz += afz * 0.035f;

			// Repulsion forces from other particles
			for( unsigned int j = 0; j < _particles.size(); ++j )
			{
				if( j == i ) continue;
				
				Particle &p2 = _particles[j];
				
				float dist2 = sqrtf( (p.px - p2.px)*(p.px - p2.px) + (p.pz - p2.pz)*(p.pz - p2.pz) );
				float strength = 0;

				float rfx = (p.px - p2.px) / dist2;
				float rfz = (p.pz - p2.pz) / dist2;
				
				// Use three zones with different repulsion strengths
				if( dist2 <= d3 && dist2 > d2 )
				{
					float m = (f3 - 0) / (d2 - d3);
					float t = 0 - m * d3;
					strength = m * dist2 + t;
				}
				else if( dist2 <= d2 && dist2 > d1 )
				{
					float m = (f2 - f3) / (d1 - d2);
					float t = f3 - m * d2;
					strength = m * dist2 + t;
				}
				else if( dist2 <= d1 )
				{
					float m = (f1 - f2) / (0 - d1);
					float t = f2 - m * d1;
					strength = m * dist2 + t;
				}

				p.fx += rfx * strength; p.fz += rfz * strength;
			}
		}
		else
		{
			// Choose new destination
			chooseDestination( p );
		}

		// Make movement framerate independent
		p.fx *= (30 / fps);
		p.fz *= (30 / fps);
		float vel = sqrtf( p.fx * p.fx + p.fz * p.fz );
		
		// Set new position
		p.px += p.fx; p.pz += p.fz;
		
		// Calculate orientation
		p.ox = (p.ox + p.fx) / 2;
		p.oz = (p.oz + p.fz) / 2;

		// Get rotation from orientation
		float ry = 0;
		if( p.oz != 0 ) ry = atan2( p.ox, p.oz );
		ry *= 180 / 3.1415f;	// Convert from radians to degrees
		
		// Update character scene node position
		h3dSetNodeTransform( p.node, p.px, 0.02f, p.pz, 0, ry, 0, 1, 1, 1 );
		
		// Update animation
		p.animTime += vel * 35.0f;
		h3dSetModelAnimParams( p.node, 0, p.animTime, 1.0f );
	}
#endif
}
